#include <ros/ros.h>
#include <rosbag/bag.h>
#include <std_msgs/Int32.h>
#include "kaist_publisher/altimeter_converter.h"
#include "kaist_publisher/encoder_converter.h"
#include "kaist_publisher/fog_converter.h"
#include "kaist_publisher/gps_converter.h"
#include "kaist_publisher/imu_converter.h"
#include "kaist_publisher/sick_converter.h"
#include "kaist_publisher/stereo_converter.h"
#include "kaist_publisher/velodyne_converter.h"
#include "kaist_publisher/vrs_converter.h"

using namespace kaist_publisher;

enum SensorType {
    kAltimeter = 0,
    kEncoder,
    kFog,
    kGps,
    kVrs,
    kImu,
    kVelodyne,
    kStereo,
    // kSick,
    kSensorTypeCount
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "kaist_publisher");

    ros::NodeHandle nh;
    std::vector<bool> sensors(kSensorTypeCount, false);
    std::vector<std::string> sensor_names = {
        "altimeter", "encoder", "fog", "gps", "vrs", "imu", "velodyne",
        "stereo"  // ,
        // "sick"
    };

    for (size_t i = 0; i < sensors.size(); ++i) {
        bool sensor_on = false;
        nh.getParam(sensor_names[i], sensor_on);
        sensors[i] = sensor_on;
    }

    std::string dataset;
    nh.getParam("dataset", dataset);
    std::string save_to;
    nh.getParam("save_to", save_to);

    std::string altimeter_topic;
    nh.getParam("altimeter_topic", altimeter_topic);
    AltimeterConverter altimeter(nh, dataset, save_to, altimeter_topic);

    std::string encoder_topic;
    nh.getParam("encoder_topic", encoder_topic);
    EncoderConverter encoder(nh, dataset, save_to, encoder_topic);

    std::string fog_topic;
    nh.getParam("fog_topic", fog_topic);
    FogConverter fog(nh, dataset, save_to, fog_topic);

    std::string gps_topic;
    nh.getParam("gps_topic", gps_topic);
    GpsConverter gps(nh, dataset, save_to, gps_topic);

    std::string vrs_topic;
    nh.getParam("vrs_topic", vrs_topic);
    VrsConverter vrs(nh, dataset, save_to, vrs_topic);

    std::string irp_topic, raw_topic, mag_topic;
    nh.getParam("imu_irp_topic", irp_topic);
    nh.getParam("imu_raw_topic", raw_topic);
    nh.getParam("imu_mag_topic", mag_topic);
    ImuConverter imu(nh, dataset, save_to, irp_topic, raw_topic, mag_topic);

    std::string vlp_left_topic, vlp_right_topic;
    nh.getParam("velodyne_left_topic", vlp_left_topic);
    nh.getParam("velodyne_right_topic", vlp_right_topic);
    VelodyneConverter vlp(nh, dataset, save_to, vlp_left_topic,
                          vlp_right_topic);

    std::string stereo_left_topic, stereo_right_topic;
    nh.getParam("stereo_left_topic", stereo_left_topic);
    nh.getParam("stereo_right_topic", stereo_right_topic);
    StereoConverter stereo(nh, dataset, save_to, stereo_left_topic,
                           stereo_right_topic);

    // std::string sick_back_topic, sick_middle_topic;
    // nh.getParam("sick_back_topic", sick_back_topic);
    // nh.getParam("sick_middle_topic", sick_middle_topic);
    // SickConverter sick(nh, dataset, save_to, sick_back_topic,
    // sick_middle_topic);

    int publish_delay;
    nh.getParam("publish_delay", publish_delay);
    publish_delay = publish_delay <= 0 ? 1 : publish_delay;

    std::vector<int64_t> timestamp_sensor(sensors.size(), LONG_MAX);
    if (sensors[0])
        timestamp_sensor[0] = altimeter.Init_Publish();
    if (sensors[1])
        timestamp_sensor[1] = encoder.Init_Publish();
    if (sensors[2])
        timestamp_sensor[2] = fog.Init_Publish();
    if (sensors[3])
        timestamp_sensor[3] = gps.Init_Publish();
    if (sensors[4])
        timestamp_sensor[4] = vrs.Init_Publish();
    if (sensors[5])
        timestamp_sensor[5] = imu.Init_Publish();
    if (sensors[6])
        timestamp_sensor[6] = vlp.Init_Publish();
    if (sensors[7])
        timestamp_sensor[7] = stereo.Init_Publish();

    ros::Rate r(1000.0 / publish_delay);
    int cur_idx =
        int(min_element(timestamp_sensor.begin(), timestamp_sensor.end()) -
            timestamp_sensor.begin());
    while (timestamp_sensor[cur_idx] != LONG_MAX) {
        if (cur_idx == 0)
            timestamp_sensor[cur_idx] = altimeter.Publish();
        else if (cur_idx == 1)
            timestamp_sensor[cur_idx] = encoder.Publish();
        else if (cur_idx == 2)
            timestamp_sensor[cur_idx] = fog.Publish();
        else if (cur_idx == 3)
            timestamp_sensor[cur_idx] = gps.Publish();
        else if (cur_idx == 4)
            timestamp_sensor[cur_idx] = vrs.Publish();
        else if (cur_idx == 5)
            timestamp_sensor[cur_idx] = imu.Publish();
        else if (cur_idx == 6)
            timestamp_sensor[cur_idx] = vlp.Publish();
        else if (cur_idx == 7)
            timestamp_sensor[cur_idx] = stereo.Publish();

        cur_idx =
            int(min_element(timestamp_sensor.begin(), timestamp_sensor.end()) -
                timestamp_sensor.begin());
        r.sleep();
    }

    return 0;
}